# -*- coding: utf-8 -*-
"""
Created on Wed Jun 25 16:40:05 2014

@author: Maxim
"""

import aircraft
from FlightConditions import ISAtmosphere
import convert

import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import interp1d


def get_hinge_derivatives(chordRatio):
    """
    calculate dCh/dalpha, dCh/delev etc.
    """
    _chordRatio = np.array([0.0, 0.064142, 0.147500, 0.273531, 0.416778, 0.609324, 0.808278, 1.0])
    _ChdDelta = np.array([0.0, -0.005036, -0.008766, -0.012467, -0.015343, -0.018889, -0.022217, -0.025265])
    curve = interp1d(_chordRatio, _ChdDelta,'cubic')
    k = _ChdDelta[-1]
    ChdDelta = curve(chordRatio)
    ChdAlpha = chordRatio*k
    return ChdDelta, ChdAlpha


def get_CLde(chordRatio):
    """ Figure 12:4 """
    _coef = np.array([0.0, 0.022441, 0.034331, 0.044059, 0.054873, 0.061538, 0.066213, 0.069563, 0.071251, 0.071868])
    _cr = np.array([0.0, 0.136181, 0.227375, 0.303276, 0.405573, 0.509740, 0.619405, 0.743567, 0.857312, 1.0])
    curve = interp1d(_cr,_coef,'cubic')
    return curve(chordRatio)

def control_force(ac,aero,n,altitude,mass,cgMAC,gearRatio):
    # assumptions
    V = 50.0
    Dprop=1.73 # propeller diameter
    #---
    S = ac.wing.area
    St = ac.hStab.area
    be = ac.hStab.span
    ce = ac.hStab.elevator.avgChord
    TR = ac.wing.taper
    cgX = ac.wing.get_fs_on_mac(cgMAC)
    x = cgX - aero.xNP
    l = (ac.hStab.aapex[0]+0.25*ac.hStab.MAC) - (ac.wing.aapex[0]+0.25*ac.hStab.MAC)
    g = 9.81
    W = mass*g
    atm = ISAtmosphere(altitude)
    rho = atm.density
    cfc = ac.hStab.elevator.avgChordRatio
    AR = ac.wing.aspectRatio
    c = ac.wing.MAC
    Dprop = 1.
    #---
    CLa = aero.derivs.CLa # 1/rad
    CLa *= np.pi/180. # 1/deg
    CLtde = get_CLde(cfc)
    CheDdelta, CheDalpha = get_hinge_derivatives(cfc)
    #---
    deda = 20.*CLa*TR**0.3/(AR**(0.725))*(3.*c/l)**0.25
    
    q = rho*V*V/2.
    CLreq = W/q/S
    Treq = (aero.Cd0 + aero.k*CLreq**2.0)*q*S
    CT = Treq/(rho*V*V*Dprop)
    qtq = 1.0 + 8.*CT/np.pi
    #---
    tau = CLtde/CLa
    #---
    dHe1 = S*x*CheDdelta/(CLtde*St*l) + (1.0-deda)*qtq*CheDalpha/CLa
    dHe1 *= W/S*(n-1.0)*be*ce*ce
    
    dHe2 = 57.3*(-1.0/tau*CheDdelta + CheDalpha)*g*rho/2.0 *qtq*l*be*ce*ce*(n-1.0)
    Fe = gearRatio*(dHe1+dHe2)
    return Fe


def force_per_g(ac,aero,altitude,mass,cgMAC,gearRatio):
    n = 2.0
    Fs = control_force(ac,aero,n,altitude,mass,cgMAC,gearRatio)
    return Fs

def force_per_g_analysis():
    ac = aircraft.load('V204')
    
    gearRatio = 1.589/0.3048 #FIXME: this value is unknown and should be updated
    cgMAC    = np.array([20.,30,40])
    mass     = np.array([450.,550.,650.])
    altitude = np.array([0.,2000,4500])

    aero = ac.analyze_aero_trim()
    Fsg = np.zeros([len(mass)*len(altitude),len(cgMAC)])
    marker = ['o','*','^']
    color = ['k','g','r','b']

    plt.figure(1)
    plt.grid(True)
    plt.hold(True)
    plt.xlabel('CG position, % of MAC')
    plt.ylabel('Fs/g, lbf/g')
    leg = list()
    for i,h in enumerate(altitude):
        for j,m in enumerate(mass):
            idx = i*len(altitude)+j
            for k,cg in enumerate(cgMAC):
                 Fs = force_per_g(ac,aero,h,m,cg,gearRatio)
                 Fsg[idx,k] = convert.N_to_lbf(Fs)
            leg.append('%.0fm %.0fkg'%(h,m))
            plt.plot(cgMAC,Fsg[idx],color[i]+'-'+marker[j],mfc=color[j])
    plt.axis([cgMAC[0],cgMAC[-1],0,None])
    plt.legend(leg)
    plt.show()


if __name__=="__main__":
    force_per_g_analysis()
    